#define encoder0PinA 20
#define encoder0PinB 21
#define encoder1PinA 2
#define encoder1PinB 3

#include "SerialModif.h"
#include <PID_v1.h>

#define AV LOW  // Marche avant
#define AR HIGH // Marche arrière

volatile long encoder0Pos = 0;  // "volatile" évite de copier la variable dans un registre temporaire
                                // et permet qu'elle soit bien modifiée à n'importe quel moment (interruptions)
volatile long encoder1Pos = 0;

volatile long newPositionG;
volatile long newPositionD;
// Sorties
int pinMoteurG = 9;
int pinMoteurD = 10;
int pinDirectionG = 11;
int pinDirectionD = 12;

//Entrées
const char pinTirette = 24;

// Initialisations
int tempsActuel = 0;
int tempsAncien = 0;

int tensionMoteurG = 0;
int tensionMoteurSigneeG = 0;
int consigneG = 0;

int tensionMoteurD = 0;
int tensionMoteurSigneeD = 0;
int consigneD = 0;

float kp = 0.05;           // Coefficient proportionnel
float ki = 0.0012;           // Coefficient intégrateur
float kd = 0.00015;           // Coefficient dérivateur


double SetpointG, InputG, OutputG;
PID PID_G(&InputG, &OutputG, &SetpointG,kp,ki,kd, DIRECT);

double SetpointD, InputD, OutputD;
PID PID_D(&InputD, &OutputD, &SetpointD,kp,ki,kd, DIRECT);

SerialModif serialM; // Objet servant à modifier des variables par le port série

void setup() {

  pinMode(encoder0PinA, INPUT);
  pinMode(encoder0PinB, INPUT);
  pinMode(encoder1PinA, INPUT);
  pinMode(encoder1PinB, INPUT);
  
  pinMode(pinTirette, INPUT_PULLUP);  //initialise en entrée la pin de la tirette (avec resistance de pullup)
  //digitalWrite(encoder1PinA, HIGH);
  //digitalWrite(encoder1PinB, HIGH);

  attachInterrupt(3, doEncoderA, FALLING);
  attachInterrupt(4, doEncoderB, FALLING);
  attachInterrupt(0, doEncoderA2, FALLING);
  attachInterrupt(1, doEncoderB2, FALLING);

  pinMode(pinMoteurG,OUTPUT);
  pinMode(pinMoteurD,OUTPUT);
  pinMode(pinDirectionG,OUTPUT);
  pinMode(pinDirectionD,OUTPUT);

  digitalWrite(pinDirectionG, AV);
  digitalWrite(pinDirectionD, AV);

  newPositionG =0;
  newPositionD =0;

  InputG = newPositionG;
  SetpointG = consigneG;
  PID_G.SetOutputLimits(-255, 255);
  PID_G.SetMode(AUTOMATIC);
  PID_G.SetSampleTime(200);

  InputD = newPositionD;
  SetpointD = consigneD;
  PID_D.SetOutputLimits(-255, 255);
  PID_D.SetMode(AUTOMATIC);
  PID_D.SetSampleTime(200);

  serialM.begin(/*115200*/);
  serialM.set("Ki = ", 'i', &ki);
  serialM.set("Kp = ", 'p', &kp);
  serialM.set("Kd = ", 'd', &kd);
  /*serialM.set("PosG: ", '\0', &newPositionG);
  serialM.set("PosD: ", '\0', &newPositionD);
  serialM.set("TensionSG: ", '\0', &tensionMoteurSigneeG);
  serialM.set("TensionSD: ", '\0', &tensionMoteurSigneeD);
  serialM.set("TensionG: ", '\0', &tensionMoteurG);
  serialM.set("TensionD: ", '\0', &tensionMoteurD);*/
  
}

void loop()
{
  serialM.change(); // modifie les variables si l'utilisateur l'a demandé

  newPositionG = encoder0Pos/10;
  newPositionD = (encoder1Pos)/10;

  InputG = newPositionG;
  SetpointG = consigneG;
  PID_G.SetTunings(kp, ki, kd);
  PID_G.Compute();

  InputD = newPositionD;
  SetpointD = consigneD;
  PID_D.SetTunings(kp, ki, kd);
  PID_D.Compute();

  tempsActuel = millis();

  if (tempsActuel - tempsAncien >= 200) // actualise la vitesse toutes les 100ms
  {
    tempsAncien = tempsActuel;
    
    serialM.print(); // affiche

    // *****change la tension du moteur*****
    //tensionMoteurSigneeG += (consigneG - newPositionGGG)/100; //avec une accélération propostionnelle
    tensionMoteurSigneeG = OutputG;
    tensionMoteurSigneeD = OutputD;

    if(tensionMoteurSigneeG>0)
    {
        digitalWrite(pinDirectionG, AR);
    }
    else
    {
        digitalWrite(pinDirectionG, AV);
    }

    if(tensionMoteurSigneeD>0)
      digitalWrite(pinDirectionD, AR);
    else
      digitalWrite(pinDirectionD, AV);

    tensionMoteurG = abs(tensionMoteurSigneeG);
    if(tensionMoteurG>255)
      tensionMoteurG = 255;
    if(tensionMoteurG<0)
      tensionMoteurG = 0;

    tensionMoteurD = abs(tensionMoteurSigneeD);
    if(tensionMoteurD>255)
      tensionMoteurD = 255;
    if(tensionMoteurD<0)
      tensionMoteurD = 0;

    //constokjiorain(tensionMoteurG, 0, 255); // la valeur est reste entre 0 et 255

    // actualise les sorties
    analogWrite(pinMoteurG,tensionMoteurG);
    analogWrite(pinMoteurD,tensionMoteurD);
  }
}


void doEncoderA(){

  // look for a low-to-high on channel A
  if (digitalRead(encoder0PinA) == HIGH) {

    // check channel B to see which way encoder is turning
    if (digitalRead(encoder0PinB) == LOW) {
      encoder0Pos = encoder0Pos + 1;         // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;         // CCW
    }
  }

  else   // must be a high-to-low edge on channel A
  {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder0PinB) == HIGH) {
      encoder0Pos = encoder0Pos + 1;          // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }
  }
  //Serial.println (encoder0Pos, DEC);
  // use for debugging - remember to comment out

}

void doEncoderB(){

  // look for a low-to-high on channel B
  if (digitalRead(encoder0PinB) == HIGH) {

    // check channel A to see which way encoder is turning
    if (digitalRead(encoder0PinA) == HIGH) {
      encoder0Pos = encoder0Pos + 1;         // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;         // CCW
    }
  }

  // Look for a high-to-low on channel B

  else {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder0PinA) == LOW) {
      encoder0Pos = encoder0Pos + 1;          // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }
  }

}
void doEncoderA2(){

  // look for a low-to-high on channel A
  if (digitalRead(encoder1PinA) == HIGH) {

    // check channel B to see which way encoder is turning
    if (digitalRead(encoder1PinB) == LOW) {
      encoder1Pos = encoder1Pos + 1;         // CW
    }
    else {
      encoder1Pos = encoder1Pos - 1;         // CCW
    }
  }

  else   // must be a high-to-low edge on channel A
  {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder1PinB) == HIGH) {
      encoder1Pos = encoder1Pos + 1;          // CW
    }
    else {
      encoder1Pos = encoder1Pos - 1;          // CCW
    }
  }
  //Serial.println (encoder0Pos, DEC);
  // use for debugging - remember to comment out

}

void doEncoderB2(){

  // look for a low-to-high on channel B
  if (digitalRead(encoder1PinB) == HIGH) {

    // check channel A to see which way encoder is turning
    if (digitalRead(encoder1PinA) == HIGH) {
      encoder1Pos = encoder1Pos + 1;         // CW
    }
    else {
      encoder1Pos = encoder1Pos - 1;         // CCW
    }
  }

  // Look for a high-to-low on channel B

  else {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder1PinA) == LOW) {
      encoder1Pos = encoder1Pos + 1;          // CW
    }
    else {
      encoder1Pos = encoder1Pos - 1;          // CCW
    }
  }

}
